#include "motorsTask.h"

#include "FreeRTOS.h"
#include "task.h"

#include <math.h>
#include "nxbot.h"

#define PRINT_INFO 0
#define CONTROL_ACOPLADO 0

static void motorsTask(void *pvParameters);
//int sum(int *buffer);

void executeMotorsTask(globals_t *pvParameter)
{
	xTaskCreate(motorsTask,(const signed portCHAR * const) "motorTask",motorsTask_stackSize,(void *)pvParameter,tskIDLE_PRIORITY+2,( xTaskHandle * ) NULL);
}


//Tarea para solo actualizar las vels en la esctructura global
static void motorsTask(void *pvParameters)
{	
	//convertion of parameters 
	globals_t *globals;
	globals=(globals_t *)pvParameters;

	unsigned int freq;
	portTickType actualTick;

	//odometers direction
	unsigned char leftMotorDir = 0, rightMotorDir = 0;

	
	#if USE_ODOMETRY==1
	//variables used for odometry
	long change_left;
  long change_right;
  float transchange;
  float rotchange;
  float dx,dy;
  #endif
  
  #if USE_VCONTROL==1
  
  //error
  int errorLeft, errorRight = 0;
  int propVelLeft,propVelRight = 0;
  int intVelLeft,intVelRight = 0;
  int derVelLeft, derVelRight = 0;
  //valor integral de la anterior iteracion
  int iT0L = 0, iT0R = 0;
  int errorT0L = 0, errorT0R = 0;
  

  #if CONTROL_ACOPLADO==1
  //variables para el control de acople (sincronizacion ruedas)
  int bias, errorSync = 0;
  int psVelLeft, psVelRight = 0;
  int isVelLeft, isVelRight = 0;
  int isT0L, isT0R;
  
  //constantes de control sincronizado
  float Kps, Tis;

	//FIXME: Estos parametros deben poder ser configurables.
  Kps = 0.01f;
  Tis = 0.1f;  
  #endif
  
  int ctrlVelLeft, ctrlVelRight = 0;
  
  #if PRINT_INFO==1
  char info[50];
	#endif
  

  #endif
	
	for(;;)
	{		

		//aquire the frequency for the task		
		freq=globals->times.intervalMotors;
		
		//First update position
		motors_getActualPos(&(globals->motors));
		
		//Update velocities
		motors_getActualVels(&(globals->motors), MOTOR_INTERVAL_HZ);
		//Update direction of motors
		motors_getDirection(&leftMotorDir, &rightMotorDir)
		
		//Odometry calculation
		#if USE_ODOMETRY==1
		//calculate variations of position of wheels
		change_left = globals->encoders.posLeft - globals->encoders.lastPosLeft;
  	change_right = globals->encoders.posRight - globals->encoders.lastPosRight;
  
		//correction of encoders overflow
		if(change_left>60000)
			change_left = (globals->encoders.posLeft-65535)-globals->encoders.lastPosLeft;
	 	else if(change_left<-60000)
	 		change_left = (globals->encoders.posLeft)+(65535-globals->encoders.lastPosLeft);
	 	
	 	if(change_right>60000)
			change_right = (globals->encoders.posRight-65535)-globals->encoders.lastPosRight;
	 	else if(change_right<-60000)
	 		change_right = (globals->encoders.posRight)+(65535-globals->encoders.lastPosRight);
 	
	 	//update last positions
		globals->encoders.lastPosLeft = globals->encoders.posLeft;
		globals->encoders.lastPosRight = globals->encoders.posRight;

		transchange = (change_left + change_right) * NXBOT_ENCODER_RES / 2;
		rotchange = (change_left - change_right) * NXBOT_ENCODER_RES / NXBOT_WHEEL_DISTANCE;

		dx = transchange*cos(-globals->grobotPose.yaw - rotchange/2);
		dy = transchange*sin(-globals->grobotPose.yaw - rotchange/2);
		
		globals->robotPose.x += dx;
		globals->robotPose.y += dy;
		globals->robotPose.yaw += rotchange;

		while (globals->robotPose.yaw > 2*M_PI) 
			globals->robotPose.yaw -= 2*M_PI;
		while (globals->robotPose.yaw < 0) 
			globals->robotPose.yaw += 2*M_PI;

		globals->robotPose.orientation = globals->robotPose.yaw;
		
		if(globals->robotPose.orientation > M_PI)
  		globals->robotPose.orientation -=2*M_PI;

  	globals->robotPose.orientation *= -1;
		#endif
		
		//define direction of odometers, in order to increment or decrement them. If motors are turned off, no change will be made to the direction.
		if(globals->motors.velocities.velLeftRef>0)
			leftMotorDir = ENC_FORWARD;
		else if(globals->motors.velocities.velLeftRef<0)
			leftMotorDir = ENC_REVERSE;
			
		if(globals->motors.velocities.velRightRef>0)
			rightMotorDir = ENC_FORWARD;
		else if (globals->motors.velocities.velRightRef<0)
			rightMotorDir = ENC_REVERSE;
		
		//Set direction in odometers
		motors_setDirection(leftMotorDir, rightMotorDir);
		
		//VELOCITY CONTROL
		#if USE_VCONTROL==1
		
		#if CONTROL_ACOPLADO==1
		//control de acople entre ruedas
		bias = globals->motors.velocities.velLeftRef - globals->motors.velocities.velRightRef;
		errorSync = bias - (globals->motors.velocities.velLeftAct - globals->motors.velocities.velRightAct);
		
		//control de acople
		if(globals->motors.velocities.velLeftRef != 0)
		{
			//Control proporcional
			psVelLeft = Kps*errorSync;
			//Control integral
			isVelLeft = errorSync*Tis + isT0L;
			isT0L = isVelLeft;
		}
		else
		{
			psVelLeft = 0;
			isVelLeft = 0;
			isT0L = 0;
		}
		
		if(globals->motors.velocities.velRightRef != 0)
		{
			//Control proporcional
			psVelRight = Kps*errorSync;
			//Control integral
			isVelRight = errorSync*Tis + isT0R;
			isT0R = isVelRight;
		}
		else
		{
			psVelRight = 0;
			isVelRight = 0;
			isT0R = 0;
		}
		
		//calculo del error
		errorLeft = globals->motors.velocities.velLeftRef - globals->motors.velocities.velLeftAct + isVelLeft + psVelLeft;
		errorRight = globals->motors.velocities.velRightRef - globals->motors.velocities.velRightAct - isVelRight - psVelRight;
		#else
		errorLeft = globals->motors.velocities.velLeftRef - globals->motors.velocities.velLeftAct;
		errorRight = globals->motors.velocities.velRightRef - globals->motors.velocities.velRightAct;
		#endif
		//control proporcional
		propVelLeft = errorLeft*globals->parametersPI.Kp;
		propVelRight = errorRight*globals->parametersPI.Kp;
		
		//control integral
		if(globals->motors.velocities.velLeftRef != 0)
		{
			intVelLeft = errorLeft*globals->parametersPI.Ti + iT0L;
			iT0L = intVelLeft;
		}
		else
		{
			intVelLeft = 0;
			iT0L = 0;
		}
		
		if(globals->motors.velocities.velRightRef != 0)
		{
			intVelRight = errorRight*globals->parametersPI.Ti + iT0R;
			iT0R = intVelRight;
		}
		else
		{
			intVelRight = 0;
			iT0R = 0;
		}
		
		//control derivativo
		derVelLeft = globals->parametersPI.Td*(errorLeft-errorT0L);
		derVelRight = globals->parametersPI.Td*(errorRight-errorT0R);
		errorT0L = errorLeft;
		errorT0R = errorRight;
				
		//Si la velocidad de referencia es cero, no actua el control de velocidad
		//motor izquierdo
		if(globals->motors.velocities.velLeftRef != 0)
			ctrlVelLeft = globals->motors.velocities.velLeftRef + propVelLeft + intVelLeft + derVelLeft;
		else
			ctrlVelLeft = 0;
		//motor derecho	
		if(globals->motors.velocities.velRightRef != 0)
			ctrlVelRight = globals->motors.velocities.velRightRef + propVelRight + intVelRight + derVelRight;
		else
			ctrlVelRight = 0;
		
		//si el control llega al limite y cambia la direccion de lavelocidad, poner a cero.
		if(globals->motors.velocities.velLeftRef<0 && ctrlVelLeft>0 )
			ctrlVelLeft = 0;
		else if (globals->motors.velocities.velLeftRef>0 && ctrlVelLeft<0 )
			ctrlVelLeft = 0;
		
		if(globals->motors.velocities.velRightRef<0 && ctrlVelRight>0 )
			ctrlVelRight = 0;
		else if (globals->motors.velocities.velRightRef>0 && ctrlVelRight<0 )
			ctrlVelRight = 0;
		
		
		#if PRINT_INFO==1
		sprintf(info,"%d,%d PL:%d PR:%d, IL:%d IR:%d-%d,%d\r\n",globals->motors.velocities.velLeftAct, globals->motors.velocities.velRightAct, propVelLeft,propVelRight,intVelLeft,intVelRight,ctrlVelLeft,ctrlVelRight);
		//sprintf(info,"%d,%d bias:%d errorSync:%d, IL:%d IR:%d-%d,%d\r\n",globals->motors.velocities.velLeftAct, globals->motors.velocities.velRightAct, bias,errorSync,isVelLeft,isVelRight,ctrlVelLeft,ctrlVelRight);
		uart1_puts(info);
		#endif
		
		//set velocity in motors (controlled velocity)
		motors_setVel(ctrlVelLeft,MOTOR_LEFT);
		motors_setVel(ctrlVelRight,MOTOR_RIGHT);
		
		#else
		//set velocity in motors (reference velocity, no control)
		motors_setVel(globals->motors.velocities.velLeftRef,MOTOR_LEFT);
		motors_setVel(globals->motors.velocities.velRightRef,MOTOR_RIGHT);
		#endif
		
		//time delay according to the encodersInterval value
		actualTick = xTaskGetTickCount();
		vTaskDelayUntil( &actualTick,(portTickType)freq);		
	}
}

